#include  "../include/zeus_s2_driver/zeus_s2_driver.h"


DIRVEDATA  dirveDate_ros;

DriverPub::DriverPub():
last_twist_time_(0),
line_speed_percent(0),
angle_speed_percent(0),
queueFlag(0),
can_recv_flag(0),
rev_left_(0),
rev_left_1_(0),
rev_right_(0),
rev_right_1_(0),
rev_flag(0),
driver_count(0){
}

DriverPub::~DriverPub()
{
    boost::mutex::scoped_lock look(mutex_);
    l_wheel_mult_ = 0;
    r_wheel_mult_ = 0;
//高配版
#ifdef  NVIDIA_XAVIER
    can_deinit();
    memset(buffer_data,0,17);
#else       //低配版
    VCI_ClearBuffer(VCI_USBCAN2,0,0);   //清空缓存
    VCI_ClearBuffer(VCI_USBCAN2,0,1);
    VCI_ResetCAN(VCI_USBCAN2,0,0);      //复位通道
    VCI_ResetCAN(VCI_USBCAN2,0,1);
    VCI_CloseDevice(VCI_USBCAN2,0);     //关闭can分析仪
#endif

    printf("DirvePub deinit.\n");

}

void DriverPub::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        current_twist_ = *msg.get();
        cmd_vel_mutex_.unlock();

    }
    catch(const std::exception& e)
    {
        cmd_vel_mutex_.unlock();
        std::cerr << e.what() << '\n';
    }
}

void DriverPub::cmd_joy_callback(const zeus_s2_msgs::joy_control::ConstPtr& msg)
{
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        ps3_control_ = *msg.get();
        // joy_flag_ = true;
        cmd_vel_mutex_.unlock();

    }
    catch(const std::exception& e)
    {
        cmd_vel_mutex_.unlock();
        std::cerr << e.what() << '\n';
    }
}

	
// 使能电机驱动指令
void DriverPub::enable_motor_driver(uint16_t driver_id )
{
#ifdef  NVIDIA_XAVIER
 //需要发送的帧，结构体设置
	char tx_buff[8];
	tx_buff[0] = 0;		
	tx_buff[1] = 0x24;		
	tx_buff[2] = 0;		
	tx_buff[3] = 0x10;
	tx_buff[4] = 0;		
	tx_buff[5] = 1;		
	tx_buff[6] = 0;		
	tx_buff[7] = 0;		
    if( can0_write(driver_id, tx_buff) == 1)
	{

	}
	else
	{
			printf("can1:send data failed!");
	}
	

#else
	int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ send[1];
	send[0].ID = driver_id;  //0x101  102  103  104
	send[0].SendType=0;
	send[0].RemoteFlag=0;
	send[0].ExternFlag=0;		//标准帧
	send[0].DataLen=8;
	
	send[0].Data[0] = 0;		
	send[0].Data[1] = 0x24;		//0x1E 写使能保存了
	send[0].Data[2] = 0;		
	send[0].Data[3] = 0x10;
	send[0].Data[4] = 0;		
	send[0].Data[5] = 1;		
	send[0].Data[6] = 0;		
	send[0].Data[7] = 0;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
	{
		//printf("Index:%04d  ",count);count++;
		// printf("CAN1 TX ID:0x%08X",send[0].ID);
		// if(send[0].ExternFlag==0) printf(" Standard ");
		// if(send[0].ExternFlag==1) printf(" Extend   ");
		// if(send[0].RemoteFlag==0) printf(" Data   ");
		// if(send[0].RemoteFlag==1) printf(" Remote ");
		// printf("DLC:0x%02X",send[0].DataLen);
		// printf(" data:0x");

		// for(i=0;i<send[0].DataLen;i++)
		// {
		// 	printf(" %02X",send[0].Data[i]);
		// }

		// printf("\n");
		//send[0].ID+=1;
	}
	else
	{
		printf("can1:send data failed!");
	}

#endif


}

// 失能电机驱动指令
void DriverPub::disable_motor_driver(uint16_t driver_id )
{
#ifdef  NVIDIA_XAVIER
 //需要发送的帧，结构体设置
	char tx_buff[8];
	tx_buff[0] = 0;		
	tx_buff[1] = 0x1E;		
	tx_buff[2] = 0;		
	tx_buff[3] = 0x10;
	tx_buff[4] = 0;		
	tx_buff[5] = 0;		
	tx_buff[6] = 0;		
	tx_buff[7] = 0;		
    if( can0_write(driver_id, tx_buff) == 1)
	{

	}
	else
	{
			printf("can1:send data failed!");
	}
	

#else
	int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ send[1];
	send[0].ID = driver_id;  //0x101  102  103  104
	send[0].SendType=0;
	send[0].RemoteFlag=0;
	send[0].ExternFlag=0;		//标准帧
	send[0].DataLen=8;
	
	send[0].Data[0] = 0;		
	send[0].Data[1] = 0x1E;		//0x1E 写使能保存了
	send[0].Data[2] = 0;		
	send[0].Data[3] = 0x10;
	send[0].Data[4] = 0;		
	send[0].Data[5] = 0;		
	send[0].Data[6] = 0;		
	send[0].Data[7] = 0;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
	{
		//printf("Index:%04d  ",count);count++;
		// printf("CAN1 TX ID:0x%08X",send[0].ID);
		// if(send[0].ExternFlag==0) printf(" Standard ");
		// if(send[0].ExternFlag==1) printf(" Extend   ");
		// if(send[0].RemoteFlag==0) printf(" Data   ");
		// if(send[0].RemoteFlag==1) printf(" Remote ");
		// printf("DLC:0x%02X",send[0].DataLen);
		// printf(" data:0x");

		// for(i=0;i<send[0].DataLen;i++)
		// {
		// 	printf(" %02X",send[0].Data[i]);
		// }

		// printf("\n");
		//send[0].ID+=1;
	}
	else
	{
		printf("can1:send data failed!");
	}

#endif
}

// 清除驱动器报警指令 失能----》清除
void DriverPub::clear_ALM_motor_driver(uint16_t driver_id )
{
#ifdef  NVIDIA_XAVIER
 //需要发送的帧，结构体设置
	char tx_buff[8];
	tx_buff[0] = 0;		
	tx_buff[1] = 0x21;		
	tx_buff[2] = 0;		
	tx_buff[3] = 0x10;
	tx_buff[4] = 0;		
	tx_buff[5] = 0;		
	tx_buff[6] = 0;		
	tx_buff[7] = 1;		
    if( can0_write(driver_id, tx_buff) == 1)
	{

	}
	else
	{
		printf("can1:send data failed!");
	}
	

#else
	int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ send[1];
	send[0].ID = driver_id;  //0x101  102  103  104
	send[0].SendType=0;
	send[0].RemoteFlag=0;
	send[0].ExternFlag=0;		//标准帧
	send[0].DataLen=8;
	
	send[0].Data[0] = 0;		
	send[0].Data[1] = 0x21;		
	send[0].Data[2] = 0;		
	send[0].Data[3] = 0x10;
	send[0].Data[4] = 0;		
	send[0].Data[5] = 0;		
	send[0].Data[6] = 0;		
	send[0].Data[7] = 1;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
	{
		//printf("Index:%04d  ",count);count++;
		// printf("CAN1 TX ID:0x%08X",send[0].ID);
		// if(send[0].ExternFlag==0) printf(" Standard ");
		// if(send[0].ExternFlag==1) printf(" Extend   ");
		// if(send[0].RemoteFlag==0) printf(" Data   ");
		// if(send[0].RemoteFlag==1) printf(" Remote ");
		// printf("DLC:0x%02X",send[0].DataLen);
		// printf(" data:0x");

		// for(i=0;i<send[0].DataLen;i++)
		// {
		// 	printf(" %02X",send[0].Data[i]);
		// }

		// printf("\n");
		//send[0].ID+=1;
	}
	else
	{
		printf("can1:send data failed!");
	}
#endif
}


//设置电机为转距模式
void DriverPub::Set_motor_torque_mode(uint16_t driver_id )
{
#ifdef      NVIDIA_XAVIER
	//需要发送的帧，结构体设置
	char tx_buff[8];
	tx_buff[0] = 0;		
	tx_buff[1] = 0x1E;		
	tx_buff[2] = 0;		
	tx_buff[3] = 0x03;
	tx_buff[4] = 0;		
	tx_buff[5] = 1;		
	tx_buff[6] = 0;		
	tx_buff[7] = 0;		
    if( can0_write(driver_id, tx_buff) == 1)
	{

	}
	else
	{
		printf("can1:设置电机转距模式失败send data failed!");
	}

 #else
    	int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ send[1];
	send[0].ID = driver_id;  //0x101  102  103  104
	send[0].SendType=0;
	send[0].RemoteFlag=0;
	send[0].ExternFlag=0;		//标准帧
	send[0].DataLen=8;
	
	send[0].Data[0] = 0;		
	send[0].Data[1] = 0x1E;		
	send[0].Data[2] = 0;		
	send[0].Data[3] = 0x03;
	send[0].Data[4] = 0;		
	send[0].Data[5] = 1;		
	send[0].Data[6] = 0;		
	send[0].Data[7] = 0;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
	{
		//printf("Index:%04d  ",count);count++;
		// printf("CAN1 TX ID:0x%08X",send[0].ID);
		// if(send[0].ExternFlag==0) printf(" Standard ");
		// if(send[0].ExternFlag==1) printf(" Extend   ");
		// if(send[0].RemoteFlag==0) printf(" Data   ");
		// if(send[0].RemoteFlag==1) printf(" Remote ");
		// printf("DLC:0x%02X",send[0].DataLen);
		// printf(" data:0x");

		// for(i=0;i<send[0].DataLen;i++)
		// {
		// 	printf(" %02X",send[0].Data[i]);
		// }

		// printf("\n");
		//send[0].ID+=1;
	}
	else
	{
		printf("can1:设置电机转距模式失败send data failed!");
	}
#endif


}

//设定电机的转距值
void DriverPub::Set_torque_value_to_motor(uint16_t driver_id ,uint16_t value )
{
#ifdef      NVIDIA_XAVIER
	tx_buf[0]  = 0;		
	tx_buf[1] = 0x28;		
	tx_buf[2] = 0;		
	tx_buf[3] = 0x01;
	tx_buf[4] = (value>>8)&0xFF;		
	tx_buf[5] = (value&0xFF);		
	tx_buf[6] = 0;		
	tx_buf[7] = 0;		
   
	if( can0_write(driver_id, tx_buf) == 1)
	{

	}
	else
	{
		printf("can1:设置电机转距值send data failed!");
	}

#else
    	int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ send[1];
	send[0].ID = driver_id;  //0x101  102  103  104
	send[0].SendType=0;
	send[0].RemoteFlag=0;
	send[0].ExternFlag=0;		//标准帧
	send[0].DataLen=8;
	
	send[0].Data[0] = 0;		
	send[0].Data[1] = 0x28;		
	send[0].Data[2] = 0;		
	send[0].Data[3] = 0x01;
	send[0].Data[4] = (value>>8)&0xFF;		
	send[0].Data[5] = (value&0xFF);		
	send[0].Data[6] = 0;		
	send[0].Data[7] = 0;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
	{
		//printf("Index:%04d  ",count);count++;
		// printf("CAN1 TX ID:0x%08X",send[0].ID);
		// if(send[0].ExternFlag==0) printf(" Standard ");
		// if(send[0].ExternFlag==1) printf(" Extend   ");
		// if(send[0].RemoteFlag==0) printf(" Data   ");
		// if(send[0].RemoteFlag==1) printf(" Remote ");
		// printf("DLC:0x%02X",send[0].DataLen);
		// printf(" data:0x");

		// for(i=0;i<send[0].DataLen;i++)
		// {
		// 	printf(" %02X",send[0].Data[i]);
		// }

		// printf("\n");
		//send[0].ID+=1;
	}
	else
	{
		printf("can1:设置电机转距值send data failed!");
	}


#endif

}

void DriverPub::send_speed_callback()
{
    double linear_speed, angular_speed;
 

    if((ros::Time::now() - last_twist_time_).toSec() <= 1.0)
    {

        linear_speed = current_twist_.linear.x;
        angular_speed = current_twist_.angular.z;
        // printf("linear_speed:%lf\n",linear_speed);
        // printf("angular_speed:%lf\n",angular_speed);
    } 
    else
    {
        linear_speed = 0;
        angular_speed = 0;
    }
	if(linear_speed == 0)
	{
        	right = -angular_speed*wheel_track_ /2.0;
	        left  = right;	
            // printf("right:%f\n",right);
            // printf("left:%f\n",left);
	}
	else
	{
		if(angular_speed > 0 && angular_speed < start_rotation_limit_w_ && (linear_speed > 0 && linear_speed < 0.1))
		{
			angular_speed = start_rotation_limit_w_;
		}
		if(angular_speed < 0 && angular_speed > -start_rotation_limit_w_ && (linear_speed > -0.1 && linear_speed < 0))
		{
			angular_speed = -start_rotation_limit_w_;
		}

        left  = (linear_speed - angular_speed * wheel_track_  / 2.0);
        right = -(linear_speed + angular_speed * wheel_track_ / 2.0);

	}
   
}


/*设定速度模式*/
void DriverPub::Set_motor_speed_mode(uint16_t driver_id )
{
#ifdef      NVIDIA_XAVIER

#else
       	int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ send[1];
	send[0].ID = driver_id;  //0x101  102  103  104
	send[0].SendType=0;
	send[0].RemoteFlag=0;
	send[0].ExternFlag=0;		//标准帧
	send[0].DataLen=8;
	
	send[0].Data[0] = 0;		
	send[0].Data[1] = 0x1E;		
	send[0].Data[2] = 0;		
	send[0].Data[3] = 0x03;
	send[0].Data[4] = 0;		
	send[0].Data[5] = 2;		
	send[0].Data[6] = 0;		
	send[0].Data[7] = 0;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
	{
		//printf("Index:%04d  ",count);count++;
		// printf("CAN1 TX ID:0x%08X",send[0].ID);
		// if(send[0].ExternFlag==0) printf(" Standard ");
		// if(send[0].ExternFlag==1) printf(" Extend   ");
		// if(send[0].RemoteFlag==0) printf(" Data   ");
		// if(send[0].RemoteFlag==1) printf(" Remote ");
		// printf("DLC:0x%02X",send[0].DataLen);
		// printf(" data:0x");

		// for(i=0;i<send[0].DataLen;i++)
		// {
		// 	printf(" %02X",send[0].Data[i]);
		// }

		// printf("\n");
		//send[0].ID+=1;
	}
	else
	{
		printf("can1:设置电机速度模式send data failed!");
	}

#endif
 
}

/*给定速度值*/
void DriverPub::Set_speed_value_to_motor(uint16_t driver_id ,uint16_t value )
{
#ifdef  NVIDIA_XAVIER

#else
    int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ send[1];
	send[0].ID = driver_id;  //0x101  102  103  104
	send[0].SendType=0;
	send[0].RemoteFlag=0;
	send[0].ExternFlag=0;		//标准帧
	send[0].DataLen=8;
	
	send[0].Data[0] = 0;		
	send[0].Data[1] = 0x28;		
	send[0].Data[2] = 0;		
	send[0].Data[3] = 0;
	send[0].Data[4] = (value>>8)&0xFF;		
	send[0].Data[5] = (value&0xFF);		
	send[0].Data[6] = 0;		
	send[0].Data[7] = 0;		
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
	{
		//printf("Index:%04d  ",count);count++;
		// printf("CAN1 TX ID:0x%08X",send[0].ID);
		// if(send[0].ExternFlag==0) printf(" Standard ");
		// if(send[0].ExternFlag==1) printf(" Extend   ");
		// if(send[0].RemoteFlag==0) printf(" Data   ");
		// if(send[0].RemoteFlag==1) printf(" Remote ");
		// printf("DLC:0x%02X",send[0].DataLen);
		// printf(" data:0x");

		// for(i=0;i<send[0].DataLen;i++)
		// {
		// 	printf(" %02X",send[0].Data[i]);
		// }

		// printf("\n");
		//send[0].ID+=1;
	}
	else
	{
		printf("can1:设置电机速度值send data failed!");
	}

#endif

}



#ifdef  NVIDIA_XAVIER
//采集底盘编码器值
//              前
//      601 -----602
//    左         |        右     
//      604 -----603
//              后
//
void DriverPub::Calculate_LeftAndRight_Meter(struct can_frame *p)
{
	// int temp_buff = 0;
	// temp_buff = ((temp_buff<0)? (-temp_buff):temp_buff);
    /*0x601*/
	if(p->can_id == DRIVER_ID_ONE_POSE_ACK){
        if(p->data[1]==0xCA){
            	rev_left_ = ((p->data[2]<<24)|(p->data[3]<<16)|(p->data[4]<<8)|p->data[5]);     //编码器
                motorEleVol[0] = (p->data[6]<<8)|(p->data[7]);                 //电流值 千分单位
                motorEleVol[0] = motorEleVol[0]/1000.0;
                queueFlag |= 1<<0;
                rev_flag |= 1<<0;
        }
        else if(p->data[1]==0xCB){
                motorTemperature[0] = (p->data[2]<<8)|(p->data[3]);            //电机的温度
                motorSpeed[0] = -((p->data[4]<<8)|(p->data[5]));                    //电机的速度
                driverFaultCode[0] = (p->data[6]<<8)|(p->data[7]);                 //驱动器故障码
        }
			
		// printf("rev_left_:%d\n",rev_left_);
        // printf("motorEleVol[0]:%f\n",motorEleVol[0]);
        // printf("motorSpeed[0]:%d\n",motorSpeed[0]);
        // printf("driverFaultCode[0]:%d\n",driverFaultCode[0]);
        // printf("motorTemperature[0]:%d\n",motorTemperature[0]);
        // printf("----------------------------------------------------------------\n");
	}
	if(p->can_id == DRIVER_ID_TWO_POSE_ACK){
        if(p->data[1]==0xCA){
            rev_right_ = ((p->data[2]<<24)|(p->data[3]<<16)|(p->data[4]<<8)|p->data[5]);     //编码器
            motorEleVol[1] = (p->data[6]<<8)|(p->data[7]);                 //电流值 千分单位
            motorEleVol[1] = motorEleVol[1]/1000.0;
		    queueFlag |= 1<<1;
            rev_flag |= 1<<1;
            //  now_[1] = ros::Time::now();
            // delta_time_[1] = (now_[1] - last_time_[1]).toSec();
            //  last_time_[1] = now_[1];
        }
        else if(p->data[1]==0xCB){
            motorTemperature[1] = (p->data[2]<<8)|(p->data[3]);                   //电机的温度
            motorSpeed[1] = -((p->data[4]<<8)|(p->data[5]));                   //电机的速度
            driverFaultCode[1] = (p->data[6]<<8)|(p->data[7]);          //驱动器故障码
        }
		
		// printf("rev_right_:%d\n",rev_right_);
        // printf("motorEleVol[1]:%f\n",motorEleVol[1]);
        // printf("motorSpeed[1]:%d\n",motorSpeed[1]);
        // printf("driverFaultCode[1]:%d\n",driverFaultCode[1]);
        // printf("motorTemperature[1]:%d\n",motorTemperature[1]);
        // printf("----------------------------------------------------------------\n");
	}
	if(p->can_id == DRIVER_ID_THREE_POSE_ACK){
           if(p->data[1]==0xCA){
                    rev_right_1_ = ((p->data[2]<<24)|(p->data[3]<<16)|(p->data[4]<<8)|p->data[5]);     //编码器
                    motorEleVol[2] = (p->data[6]<<8)|(p->data[7]);                 //电流值 千分单位
                    motorEleVol[2] = motorEleVol[2]/1000.0;
                    queueFlag |= 1<<2;
                    rev_flag |= 1<<2;
            }
            else if(p->data[1]==0xCB){
                    motorTemperature[2] = (p->data[2]<<8)|(p->data[3]);                   //电机的温度
                    motorSpeed[2] = -((p->data[4]<<8)|(p->data[5]));                             //电机的速度
                    driverFaultCode[2] = (p->data[6]<<8)|(p->data[7]);                           //驱动器故障码
            }
		
		// printf("rev_right_1_:%d\n",rev_right_1_);
        // printf("motorEleVol[2]:%f\n",motorEleVol[2]);
        // printf("motorSpeed[2]:%d\n",motorSpeed[2]);
        // printf("driverFaultCode[2]:%d\n",driverFaultCode[2]);
        // printf("motorTemperature[2]:%d\n",motorTemperature[2]);
        // printf("----------------------------------------------------------------\n");
	}
	if(p->can_id == DRIVER_ID_FOUR_POSE_ACK){
        if(p->data[1]==0xCA){
            	rev_left_1_ = ((p->data[2]<<24)|(p->data[3]<<16)|(p->data[4]<<8)|p->data[5]);     //编码器
                motorEleVol[3] = (p->data[6]<<8)|(p->data[7]);                 //电流值 千分单位
                motorEleVol[3] = motorEleVol[3]/1000.0;
                queueFlag |= 1<<3;
                rev_flag |= 1<<3;
        }
        else if(p->data[1]==0xCB){
                motorTemperature[3] = (p->data[2]<<8)|(p->data[3]);                   //电机的温度
                motorSpeed[3] = -((p->data[4]<<8)|(p->data[5]));                   //电机的速度
                driverFaultCode[3] = (p->data[6]<<8)|(p->data[7]);          //驱动器故障码
        }
		
		// printf("rev_left_1_:%d\n",rev_left_1_);
        // printf("motorEleVol[3]:%f\n",motorEleVol[3]);
        // printf("motorSpeed[3]:%d\n",motorSpeed[3]);
        // printf("driverFaultCode[3]:%d\n",driverFaultCode[3]);
        // printf("motorTemperature[3]:%d\n",motorTemperature[3]);
        // printf("----------------------------------------------------------------\n");
}
  if(p->can_id == DRIVER_ID_MODE_ACK){
        if(p->data[0]==0x01){
                if((p->data[1]==0x25)&&(p->data[3]==0x10)&&(p->data[5]==01)){
                    printf("1号电机使能成功\n");
                }              
        }
        else if(p->data[0]==0x02){
               if((p->data[1]==0x25)&&(p->data[3]==0x10)&&(p->data[5]==01)){
                    printf("2号电机使能成功\n");
                }               
        }
         else if(p->data[0]==0x03){
               if((p->data[1]==0x25)&&(p->data[3]==0x10)&&(p->data[5]==01)){
                    printf("3号电机使能成功\n");
                }            
        }
         else if(p->data[0]==0x04){
               if((p->data[1]==0x25)&&(p->data[3]==0x10)&&(p->data[5]==01)){
                    printf("4号电机使能成功\n");
                }     
        }  
    }  
}

/*判断模式切换是否成功
*参数：     isnot    1       进行判断
*                                0        不判断
*                  mode         运行模式/驻车模式
*返回值：1     成功
*                  0     失败
*/
bool DriverPub::mode_switch_judge(struct can_frame  *p,uint8_t mode,bool isnot)
{
    if(!isnot){
        return 0;
    }
    switch (mode)
    {
    case DRIVER_OPERATION_MODE:
        	if(p->can_id == DRIVER_ID_MODE_ACK){
                if((p->data[0]==0x01)&&(p->data[1]==0x1F)){
                      /*答者ID 01   设置接收标志位*/
                      mode_rev_flag |= (1<<0);
                }
                else if((p->data[0]==0x02)&&(p->data[1]==0x1F)){
                     /*答者ID 02   设置接收标志位*/
                     mode_rev_flag |= (1<<1);
                }
                else if((p->data[0]==0x03)&&(p->data[1]==0x1F)){
                     /*答者ID 03   设置接收标志位*/
                      mode_rev_flag |= (1<<2);
                }
                else if((p->data[0]==0x04)&&(p->data[1]==0x1F)){
                     /*答者ID 04   设置接收标志位*/
                      mode_rev_flag |= (1<<3);
                }

                if(mode_rev_flag == 0x0F){
                    return true;
                }
              
            }
        break;
    case DRIVER_PARKING_MODE:
        break;
    default:
        break;
    }

    return 0;

}

#else               //低配版
//采集底盘编码器值
//              前
//      601 -----602
//    左         |        右     
//      604 -----603
//              后
//
void DriverPub::Calculate_LeftAndRight_Meter(VCI_CAN_OBJ *p)
{
	// int temp_buff = 0;
	// temp_buff = ((temp_buff<0)? (-temp_buff):temp_buff);
    /*0x601*/
	if(p->ID == DRIVER_ID_ONE_POSE_ACK){
        if(p->Data[1]==0xCA){
            	rev_left_ = ((p->Data[2]<<24)|(p->Data[3]<<16)|(p->Data[4]<<8)|p->Data[5]);     //编码器
                motorEleVol[0] = (p->Data[6]<<8)|(p->Data[7]);                 //电流值 千分单位
                motorEleVol[0] = motorEleVol[0]/1000.0;
                queueFlag |= 1<<0;
                rev_flag |= 1<<0;
        }
        else if(p->Data[1]==0xCB){
                motorTemperature[0] = (p->Data[2]<<8)|(p->Data[3]);            //电机的温度
                motorSpeed[0] = -((p->Data[4]<<8)|(p->Data[5]));                    //电机的速度
                driverFaultCode[0] = (p->Data[6]<<8)|(p->Data[7]);                 //驱动器故障码
        }
			
		// printf("rev_left_:%d\n",rev_left_);
        // printf("motorEleVol[0]:%f\n",motorEleVol[0]);
        // printf("motorSpeed[0]:%d\n",motorSpeed[0]);
        // printf("driverFaultCode[0]:%d\n",driverFaultCode[0]);
        // printf("motorTemperature[0]:%d\n",motorTemperature[0]);
        // printf("----------------------------------------------------------------\n");
	}
	if(p->ID == DRIVER_ID_TWO_POSE_ACK){
        if(p->Data[1]==0xCA){
            rev_right_ = ((p->Data[2]<<24)|(p->Data[3]<<16)|(p->Data[4]<<8)|p->Data[5]);     //编码器
            motorEleVol[1] = (p->Data[6]<<8)|(p->Data[7]);                 //电流值 千分单位
            motorEleVol[1] = motorEleVol[1]/1000.0;
		    queueFlag |= 1<<1;
            rev_flag |= 1<<1;
            //  now_[1] = ros::Time::now();
            // delta_time_[1] = (now_[1] - last_time_[1]).toSec();
            //  last_time_[1] = now_[1];
        }
        else if(p->Data[1]==0xCB){
            motorTemperature[1] = (p->Data[2]<<8)|(p->Data[3]);                   //电机的温度
            motorSpeed[1] = -((p->Data[4]<<8)|(p->Data[5]));                   //电机的速度
            driverFaultCode[1] = (p->Data[6]<<8)|(p->Data[7]);          //驱动器故障码
        }
		
		// printf("rev_right_:%d\n",rev_right_);
        // printf("motorEleVol[1]:%f\n",motorEleVol[1]);
        // printf("motorSpeed[1]:%d\n",motorSpeed[1]);
        // printf("driverFaultCode[1]:%d\n",driverFaultCode[1]);
        // printf("motorTemperature[1]:%d\n",motorTemperature[1]);
        // printf("----------------------------------------------------------------\n");
	}
	if(p->ID == DRIVER_ID_THREE_POSE_ACK){
           if(p->Data[1]==0xCA){
                    rev_right_1_ = ((p->Data[2]<<24)|(p->Data[3]<<16)|(p->Data[4]<<8)|p->Data[5]);     //编码器
                    motorEleVol[2] = (p->Data[6]<<8)|(p->Data[7]);                 //电流值 千分单位
                    motorEleVol[2] = motorEleVol[2]/1000.0;
                    queueFlag |= 1<<2;
                    rev_flag |= 1<<2;
            }
            else if(p->Data[1]==0xCB){
                    motorTemperature[2] = (p->Data[2]<<8)|(p->Data[3]);                   //电机的温度
                    motorSpeed[2] = -((p->Data[4]<<8)|(p->Data[5]));                             //电机的速度
                    driverFaultCode[2] = (p->Data[6]<<8)|(p->Data[7]);                           //驱动器故障码
            }
		
		// printf("rev_right_1_:%d\n",rev_right_1_);
        // printf("motorEleVol[2]:%f\n",motorEleVol[2]);
        // printf("motorSpeed[2]:%d\n",motorSpeed[2]);
        // printf("driverFaultCode[2]:%d\n",driverFaultCode[2]);
        // printf("motorTemperature[2]:%d\n",motorTemperature[2]);
        // printf("----------------------------------------------------------------\n");
	}
	if(p->ID == DRIVER_ID_FOUR_POSE_ACK){
        if(p->Data[1]==0xCA){
            	rev_left_1_ = ((p->Data[2]<<24)|(p->Data[3]<<16)|(p->Data[4]<<8)|p->Data[5]);     //编码器
                motorEleVol[3] = (p->Data[6]<<8)|(p->Data[7]);                 //电流值 千分单位
                motorEleVol[3] = motorEleVol[3]/1000.0;
                queueFlag |= 1<<3;
                rev_flag |= 1<<3;
        }
        else if(p->Data[1]==0xCB){
                motorTemperature[3] = (p->Data[2]<<8)|(p->Data[3]);                   //电机的温度
                motorSpeed[3] = -((p->Data[4]<<8)|(p->Data[5]));                   //电机的速度
                driverFaultCode[3] = (p->Data[6]<<8)|(p->Data[7]);          //驱动器故障码
        }
		
		// printf("rev_left_1_:%d\n",rev_left_1_);
        // printf("motorEleVol[3]:%f\n",motorEleVol[3]);
        // printf("motorSpeed[3]:%d\n",motorSpeed[3]);
        // printf("driverFaultCode[3]:%d\n",driverFaultCode[3]);
        // printf("motorTemperature[3]:%d\n",motorTemperature[3]);
        // printf("----------------------------------------------------------------\n");
	}

    if(p->ID == DRIVER_ID_MODE_ACK){
        if(p->Data[0]==0x01){
                if((p->Data[1]==0x25)&&(p->Data[3]==0x10)&&(p->Data[5]==01)){
                    printf("1号电机使能成功\n");
                }              
        }
        else if(p->Data[0]==0x02){
               if((p->Data[1]==0x25)&&(p->Data[3]==0x10)&&(p->Data[5]==01)){
                    printf("2号电机使能成功\n");
                }               
        }
         else if(p->Data[0]==0x03){
               if((p->Data[1]==0x25)&&(p->Data[3]==0x10)&&(p->Data[5]==01)){
                    printf("3号电机使能成功\n");
                }            
        }
         else if(p->Data[0]==0x04){
               if((p->Data[1]==0x25)&&(p->Data[3]==0x10)&&(p->Data[5]==01)){
                    printf("4号电机使能成功\n");
                }     
        }  
    }  
}

/*判断模式切换是否成功
*参数：     isnot    1       进行判断
*                                0        不判断
*                  mode         运行模式/驻车模式
*返回值：1     成功
*                  0     失败
*/
bool DriverPub::mode_switch_judge(VCI_CAN_OBJ *p,uint8_t mode,bool isnot)
{
    if(!isnot){
        return 0;
    }
    switch (mode)
    {
    case DRIVER_OPERATION_MODE:
        	if(p->ID == DRIVER_ID_MODE_ACK){
                if((p->Data[0]==0x01)&&(p->Data[1]==0x1F)){
                      /*答者ID 01   设置接收标志位*/
                      mode_rev_flag |= (1<<0);
                }
                else if((p->Data[0]==0x02)&&(p->Data[1]==0x1F)){
                     /*答者ID 02   设置接收标志位*/
                     mode_rev_flag |= (1<<1);
                }
                else if((p->Data[0]==0x03)&&(p->Data[1]==0x1F)){
                     /*答者ID 03   设置接收标志位*/
                      mode_rev_flag |= (1<<2);
                }
                else if((p->Data[0]==0x04)&&(p->Data[1]==0x1F)){
                     /*答者ID 04   设置接收标志位*/
                      mode_rev_flag |= (1<<3);
                }

                if(mode_rev_flag == 0x0F){
                    return true;
                }
              
            }
        break;
    case DRIVER_PARKING_MODE:
        break;
    default:
        break;
    }

    return 0;

}

// 处理上报来的底盘数据  Process the data submitted by the chassis
void DriverPub::dirve_ProcessData(VCI_CAN_OBJ *p,DIRVEDATA *PdirveDate )
{
	
	Calculate_LeftAndRight_Meter(p);    /*采集上报的编码器、转速、电流等信息*/
    mode_switch_judge(p,switching_mode,mode_isnot);
    handle_speed_data(p->ID);                /*将接收的数据进行处理*/
}

#endif



uint8_t DriverPub::get_recvFlag(void)
{
	return can_recv_flag;

}

// int32_t DriverPub::get_rev_leftVol(void)
// {
// 	return rev_left_;
// }


// int32_t DriverPub::get_rev_rightVol(void)
// {
// 	return rev_right_;
// }

void DriverPub::handle_speed_data(uint16_t number)
{
    if(start_flag_)
    {
        accumulation_y_ = 0.0;
        accumulation_x_ = 0.0;
        accumulation_th_ = 0.0;

		accumulation_left_ = 0.0;
    	accumulation_right_ = 0.0;
		accumulation_right_1_ = 0.0;
		accumulation_left_1_ = 0.0;


        cur_left_ = rev_left_;
		cur_left_1_ = rev_left_1_;
        cur_right_= rev_right_;
		cur_right_1_ = rev_right_1_;

		delta_left_ = 0;
        delta_right_ = 0;
		delta_left_1_= 0;
        delta_right_1_= 0;

        last_time_[0] = now_[0];
        last_time_[1] = now_[1];
        last_time_[2] = now_[2];
        last_time_[3] = now_[3];
        start_flag_ = false;
        ROS_INFO("ROBOT is Successfuly connected!");
        ROS_INFO_STREAM("The Code Version is  " << code_version_);

    }
     if(cur_left_ == 0 && cur_right_ == 0)
    {
        delta_left_ = 0;
        delta_right_ = 0;
		delta_left_1_= 0;
        delta_right_1_= 0;
    }

    switch(number)
    {
        case DRIVER_ID_ONE_POSE_ACK:
                if((rev_flag & 0x01)==0)
                {
                    return;
                }
                now_[0] = ros::Time::now();
                // delta_time_[0] = (now_[0] - last_time_[0]).toSec();
  
                if(rev_left_< encoder_low_wrap_ && cur_left_ > encoder_high_wrap_)
                {
                    l_wheel_mult_++;
                }
                else if(rev_left_ > encoder_high_wrap_ && cur_left_ < encoder_low_wrap_)
                {
                    l_wheel_mult_--;
                }
                else
                {
                    l_wheel_mult_ = 0;
                }
                delta_left_ = 1.0*(rev_left_ + (l_wheel_mult_ * (encoder_max_ - encoder_min_)-cur_left_))/ticks_per_meter_;                 //转换为米单位
            //    delta_left_ = 1.0*(rev_left_ + (l_wheel_mult_ * (encoder_max_ - encoder_min_)-cur_left_))/10000.0; 
                // printf("delta_left_: %lf \n",delta_left_);
                accumulation_left_ += (delta_left_/(wheel_diameter_4wd_/2.0));              //计算轮子的转动角度
                // printf("accumulation_left_: %lf \n",accumulation_left_);
                // left_right_wheel_state_.front_left_joint.position  = accumulation_left_;    //用于发布的msg
                cur_left_ = rev_left_;                                                                                                              //保存当前编码器位置
                left_right_wheel_state_.front_left_encoder.velocity =  (motorSpeed[0]/gear_reduction_4wd_*wheel_diameter_4wd_*PI/60.0);   /*转速/减速比*轮子直径*PI / 60    单位 速度 m/s*/
                // printf("front_left_encoder.velocity:%lf\n",left_right_wheel_state_.front_left_encoder.velocity);
                // last_time_[0] = now_[0];                                                                                                                  //保存时间
                rev_flag &= ~(1<<0);                                                                                                                           //清标志
        break;
        case DRIVER_ID_TWO_POSE_ACK:
                 if((rev_flag & 0x02) ==0)
                {
                    return;
                }
                now_[1] = ros::Time::now();
                // delta_time_[1] = (now_[1] - last_time_[1]).toSec();
                if(rev_right_ < encoder_low_wrap_ && cur_right_ > encoder_high_wrap_)
                {
                    r_wheel_mult_++;
                }  
                else if(rev_right_ > encoder_high_wrap_ && cur_right_ < encoder_low_wrap_)
                {
                    r_wheel_mult_--;
                }    
                else
                {
                    r_wheel_mult_ = 0;
                }
                delta_right_ = -(1.0*(rev_right_ + (r_wheel_mult_ * (encoder_max_ - encoder_min_)-cur_right_))/ticks_per_meter_);                 //转换为米单位
            //    delta_right_ = 1.0*(rev_right_ + (r_wheel_mult_ * (encoder_max_ - encoder_min_)-cur_right_))/10000.0;  
                // printf("delta_left_: %lf \n",delta_left_);
                accumulation_right_ += (delta_right_/(wheel_diameter_4wd_/2.0));              //计算轮子的转动角度
                // left_right_wheel_state_.front_right_joint.position  = accumulation_right_;    //用于发布的msg
                cur_right_ = rev_right_;                                                                                                              //保存当前编码器位置
                // left_right_wheel_state_.front_right_encoder.velocity = delta_right_ /delta_time_[1];   //速度 m/s
               left_right_wheel_state_.front_right_encoder.velocity =  (motorSpeed[1]/gear_reduction_4wd_*wheel_diameter_4wd_*PI/60.0);   //速度 m/s
                // printf("front_right_encoder.velocity:%lf\n",left_right_wheel_state_.front_right_encoder.velocity);
                // printf("转速: %lf \n", left_right_wheel_state_.front_right_encoder.velocity*60);
                // last_time_[1] = now_[1];
                rev_flag &= ~(1<<1);     
        break;
        case DRIVER_ID_THREE_POSE_ACK:
                 if((rev_flag & 0x04)==0)
                {
                    return;
                }
                now_[2] = ros::Time::now();
                 if(rev_right_1_ < encoder_low_wrap_ && cur_right_1_ > encoder_high_wrap_)
                {
                    r_wheel_mult_1_++;
                }  
                else if(rev_right_1_ > encoder_high_wrap_ && cur_right_1_ < encoder_low_wrap_)
                {
                    r_wheel_mult_1_--;
                }    
                else
                {
                    r_wheel_mult_1_ = 0;
                }
                delta_right_1_ = -(1.0*(rev_right_1_ + (r_wheel_mult_1_ * (encoder_max_ - encoder_min_)-cur_right_1_))/ticks_per_meter_);                 //转换为米单位
                // printf("delta_left_: %lf \n",delta_left_);
                accumulation_right_1_ += (delta_right_1_/(wheel_diameter_4wd_/2.0));              //计算轮子的转动角度
                // left_right_wheel_state_.rear_right_joint.position  = accumulation_right_1_;    //用于发布的msg
                cur_right_1_ = rev_right_1_;                                                                                                              //保存当前编码器位置
                left_right_wheel_state_.rear_right_encoder.velocity = (motorSpeed[2]/gear_reduction_4wd_*wheel_diameter_4wd_*PI/60.0);   //速度 m/s
                // printf("rear_right_encoder.velocity:%lf\n",left_right_wheel_state_.rear_right_encoder.velocity);
                // last_time_[2] = now_[2];
                rev_flag &= ~(1<<2);     
        break;
        case DRIVER_ID_FOUR_POSE_ACK:
                 if((rev_flag & 0x08)==0)
                {
                    return;
                }
                now_[3] = ros::Time::now();
                 if(rev_left_1_< encoder_low_wrap_ && cur_left_1_ > encoder_high_wrap_)
                {
                    l_wheel_mult_1_++;
                }
                else if(rev_left_1_ > encoder_high_wrap_ && cur_left_1_< encoder_low_wrap_)
                {
                    l_wheel_mult_1_--;
                }
                else
                {
                    l_wheel_mult_1_ = 0;
                }
                delta_left_1_ = 1.0*(rev_left_1_ + (l_wheel_mult_1_ * (encoder_max_ - encoder_min_)-cur_left_1_))/ticks_per_meter_;                 //转换为米单位
                // printf("delta_left_: %lf \n",delta_left_);
                accumulation_left_1_ += (delta_left_1_/(wheel_diameter_4wd_/2.0));              //计算轮子的转动角度
                // left_right_wheel_state_.rear_left_joint.position  = accumulation_left_1_;    //用于发布的msg
                cur_left_1_ = rev_left_1_;                                                                                                              //保存当前编码器位置
                left_right_wheel_state_.rear_left_encoder.velocity = (motorSpeed[3]/gear_reduction_4wd_*wheel_diameter_4wd_*PI/60.0);   //速度 m/s
                // printf("rear_left_encoder.velocity:%lf\n",left_right_wheel_state_.rear_left_encoder.velocity);
                // last_time_[3] = now_[3];
                rev_flag &= ~(1<<3);     
        break;
        default:
        break;

    }
}   
  

void DriverPub::receive_func()  //接收线程。
{
#ifdef    NVIDIA_XAVIER
	int reclen=0;
	int i,j;
    int ind=0;
	while(true){
        /*调用接收函数，如果有数据，进行数据处理显示。*/
		if(can0_read( &can_id)){	
					Calculate_LeftAndRight_Meter(&can_id);    /*采集上报的编码器、转速、电流等信息*/
                    mode_switch_judge(&can_id,switching_mode,mode_isnot);
                    handle_speed_data(can_id.can_id);                /*将接收的数据进行处理*/
		}
        else{
			// printf("接收超时\r\n");
		}

	}
	printf("run thread exit\n");//退出接收线程		

#else
	int reclen=0;	
	VCI_CAN_OBJ rec[3000];//接收缓存，设为3000为佳。
	int i,j;
    int ind=0;

#if 0  
        usleep(500);
         /*程序运行检测函数，对上报的缓存数据，做清理工作，程序结束，缓存数据中会有上次转速的值*/
    while(true){
        /*调用接收函数，如果有数据，进行数据处理显示。*/
        	if((reclen=VCI_Receive(VCI_USBCAN2,0,ind,rec,3000,100))>0) {
			for(j=0;j<reclen;j++){
                    Calculate_LeftAndRight_Meter(&rec[j]);			
                    if((motorSpeed[0]==0) && (motorSpeed[1]==0)&&(motorSpeed[2]==0)&&(motorSpeed[3]==0)){
                            goto  TEB;
                    }
			}
		}
    }   
TEB:	
    printf("跳出接收的初始循环\n");
#endif

	while(true){
        /*调用接收函数，如果有数据，进行数据处理显示。*/
		if((reclen=VCI_Receive(VCI_USBCAN2,0,ind,rec,3000,100))>0){
			for(j=0;j<reclen;j++){
					dirve_ProcessData(&rec[j],&dirveDate_ros);
			}
		}
        else{
			// printf("接收超时\r\n");
		}

	}
	printf("run thread exit\n");//退出接收线程		


#endif


}




//初始化机器人串口
bool DriverPub::initRobot()
{
#ifdef      NVIDIA_XAVIER
   can_init();
    printf("CAN init OK ! \n");
   //设置电机失能模式
    disable_motor_driver(DRIVER_ID_ONE_CONTROL);
    disable_motor_driver(DRIVER_ID_TWO_CONTROL);
    disable_motor_driver(DRIVER_ID_THREE_CONTROL);
    disable_motor_driver(DRIVER_ID_FOUR_CONTROL);
    //设置电机力矩模式  
    Set_motor_torque_mode(DRIVER_ID_ONE_CONTROL );
    Set_motor_torque_mode(DRIVER_ID_TWO_CONTROL );
    Set_motor_torque_mode(DRIVER_ID_THREE_CONTROL );
    Set_motor_torque_mode(DRIVER_ID_FOUR_CONTROL );
     //设定电机的力矩为零
    Set_torque_value_to_motor(DRIVER_ID_ONE_CONTROL ,0 );
    Set_torque_value_to_motor(DRIVER_ID_TWO_CONTROL ,0 );
    Set_torque_value_to_motor(DRIVER_ID_THREE_CONTROL ,0 );
    Set_torque_value_to_motor(DRIVER_ID_FOUR_CONTROL ,0 );
     //清除超时报警指令
    clear_ALM_motor_driver( DRIVER_ID_ONE_CONTROL );
    clear_ALM_motor_driver( DRIVER_ID_TWO_CONTROL );
    clear_ALM_motor_driver( DRIVER_ID_THREE_CONTROL );
    clear_ALM_motor_driver( DRIVER_ID_FOUR_CONTROL );
    sleep(1);   //等待接收线程稳定就绪
    // 设置使能电机驱动器
    enable_motor_driver(DRIVER_ID_ONE_CONTROL);
    enable_motor_driver(DRIVER_ID_TWO_CONTROL);
    enable_motor_driver(DRIVER_ID_THREE_CONTROL);
    enable_motor_driver(DRIVER_ID_FOUR_CONTROL);
    return 1;

#else
       int num=0;
	num=VCI_FindUsbDevice2(pInfo1);		//获取设备信息，返回设备数量
	printf(">>can communication init !\r\n");//指示程序已运行
	printf(">>USBCAN DEVICE NUM:");printf("%d", num);printf(" PCS");printf("\n");

		for(int i=0;i<num;i++)
		{
		printf("Device:");printf("%d", i);printf("\n");
                printf(">>Get VCI_ReadBoardInfo success!\n");
		
		printf(">>Serial_Num:%c", pInfo1[i].str_Serial_Num[0]);
		printf("%c", pInfo1[i].str_Serial_Num[1]);
		printf("%c", pInfo1[i].str_Serial_Num[2]);
		printf("%c", pInfo1[i].str_Serial_Num[3]);
		printf("%c", pInfo1[i].str_Serial_Num[4]);
		printf("%c", pInfo1[i].str_Serial_Num[5]);
		printf("%c", pInfo1[i].str_Serial_Num[6]);
		printf("%c", pInfo1[i].str_Serial_Num[7]);
		printf("%c", pInfo1[i].str_Serial_Num[8]);
		printf("%c", pInfo1[i].str_Serial_Num[9]);
		printf("%c", pInfo1[i].str_Serial_Num[10]);
		printf("%c", pInfo1[i].str_Serial_Num[11]);
		printf("%c", pInfo1[i].str_Serial_Num[12]);
		printf("%c", pInfo1[i].str_Serial_Num[13]);
		printf("%c", pInfo1[i].str_Serial_Num[14]);
	 	printf("%c", pInfo1[i].str_Serial_Num[15]);
		printf("%c", pInfo1[i].str_Serial_Num[16]);
		printf("%c", pInfo1[i].str_Serial_Num[17]);
		printf("%c", pInfo1[i].str_Serial_Num[18]);
		printf("%c", pInfo1[i].str_Serial_Num[19]);printf("\n");

		printf(">>hw_Type:%c", pInfo1[i].str_hw_Type[0]);
		printf("%c", pInfo1[i].str_hw_Type[1]);
		printf("%c", pInfo1[i].str_hw_Type[2]);
		printf("%c", pInfo1[i].str_hw_Type[3]);
		printf("%c", pInfo1[i].str_hw_Type[4]);
		printf("%c", pInfo1[i].str_hw_Type[5]);
		printf("%c", pInfo1[i].str_hw_Type[6]);
		printf("%c", pInfo1[i].str_hw_Type[7]);
		printf("%c", pInfo1[i].str_hw_Type[8]);
		printf("%c", pInfo1[i].str_hw_Type[9]);printf("\n");	

		printf(">>Firmware Version:V");
		printf("%x", (pInfo1[i].fw_Version&0xF00)>>8);
		printf(".");
		printf("%x", (pInfo1[i].fw_Version&0xF0)>>4);
		printf("%x", pInfo1[i].fw_Version&0xF);
		printf("\n");
	}
	printf(">>\n");
	printf(">>\n");
	printf(">>\n");
/*
>>can communication init !
>>USBCAN DEVICE NUM:1 PCS
Device:0
>>Get VCI_ReadBoardInfo success!
>>Serial_Num:01701020B0F
>>hw_Type:CAN-Linux
>>Firmware Version:V3.24
>>
>>
>>
*/


	if(VCI_OpenDevice(VCI_USBCAN2,0,0)==1)//打开设备
	{
		printf(">>open deivce success!\n");//打开设备成功
	}else
	{
		printf(">>open deivce error!\n");
        return 0;
		exit(1);
	}
	if(VCI_ReadBoardInfo(VCI_USBCAN2,0,&pInfo)==1)//读取设备序列号、版本等信息。
	{
                printf(">>Get VCI_ReadBoardInfo success!\n");
		
		//printf(" %08X", pInfo.hw_Version);printf("\n");
		//printf(" %08X", pInfo.fw_Version);printf("\n");
		//printf(" %08X", pInfo.dr_Version);printf("\n");
		//printf(" %08X", pInfo.in_Version);printf("\n");
		//printf(" %08X", pInfo.irq_Num);printf("\n");
		//printf(" %08X", pInfo.can_Num);printf("\n");
		printf(">>Serial_Num:%c", pInfo.str_Serial_Num[0]);
		printf("%c", pInfo.str_Serial_Num[1]);
		printf("%c", pInfo.str_Serial_Num[2]);
		printf("%c", pInfo.str_Serial_Num[3]);
		printf("%c", pInfo.str_Serial_Num[4]);
		printf("%c", pInfo.str_Serial_Num[5]);
		printf("%c", pInfo.str_Serial_Num[6]);
		printf("%c", pInfo.str_Serial_Num[7]);
		printf("%c", pInfo.str_Serial_Num[8]);
		printf("%c", pInfo.str_Serial_Num[9]);
		printf("%c", pInfo.str_Serial_Num[10]);
		printf("%c", pInfo.str_Serial_Num[11]);
		printf("%c", pInfo.str_Serial_Num[12]);
		printf("%c", pInfo.str_Serial_Num[13]);
		printf("%c", pInfo.str_Serial_Num[14]);
		printf("%c", pInfo.str_Serial_Num[15]);
		printf("%c", pInfo.str_Serial_Num[16]);
		printf("%c", pInfo.str_Serial_Num[17]);
		printf("%c", pInfo.str_Serial_Num[18]);
		printf("%c", pInfo.str_Serial_Num[19]);printf("\n");

		printf(">>hw_Type:%c", pInfo.str_hw_Type[0]);
		printf("%c", pInfo.str_hw_Type[1]);
		printf("%c", pInfo.str_hw_Type[2]);
		printf("%c", pInfo.str_hw_Type[3]);
		printf("%c", pInfo.str_hw_Type[4]);
		printf("%c", pInfo.str_hw_Type[5]);
		printf("%c", pInfo.str_hw_Type[6]);
		printf("%c", pInfo.str_hw_Type[7]);
		printf("%c", pInfo.str_hw_Type[8]);
		printf("%c", pInfo.str_hw_Type[9]);printf("\n");

		printf(">>Firmware Version:V");
		printf("%x", (pInfo.fw_Version&0xF00)>>8);
		printf(".");
		printf("%x", (pInfo.fw_Version&0xF0)>>4);
		printf("%x", pInfo.fw_Version&0xF);
		printf("\n");	
	}else
	{
		printf(">>Get VCI_ReadBoardInfo error!\n");
        return 0;
		exit(1);
	}
	
	//初始化参数，严格参数二次开发函数库说明书。
	VCI_INIT_CONFIG config;
	config.AccCode=0;
	config.AccMask=0xFFFFFFFF;
	config.Filter=1;//接收所有帧
	config.Timing0=0x00;/*波特率500 Kbps  0x00  0x1C  1000kbps  0x00  0x14*/

	config.Timing1=0x14;
	config.Mode=0;//正常模式		
	
	if(VCI_InitCAN(VCI_USBCAN2,0,0,&config)!=1)
	{
		printf(">>Init CAN1 error\n");
		VCI_CloseDevice(VCI_USBCAN2,0);
	}

	if(VCI_StartCAN(VCI_USBCAN2,0,0)!=1)
	{
		printf(">>Start CAN1 error\n");
		VCI_CloseDevice(VCI_USBCAN2,0);

	}

	if(VCI_InitCAN(VCI_USBCAN2,0,1,&config)!=1)
	{
		printf(">>Init can2 error\n");
		VCI_CloseDevice(VCI_USBCAN2,0);

	}
	if(VCI_StartCAN(VCI_USBCAN2,0,1)!=1)
	{
		printf(">>Start can2 error\n");
		VCI_CloseDevice(VCI_USBCAN2,0);

	}

    //设置电机失能模式
    disable_motor_driver(DRIVER_ID_ONE_CONTROL);
    disable_motor_driver(DRIVER_ID_TWO_CONTROL);
    disable_motor_driver(DRIVER_ID_THREE_CONTROL);
    disable_motor_driver(DRIVER_ID_FOUR_CONTROL);
    //设置电机力矩模式  
    Set_motor_torque_mode(DRIVER_ID_ONE_CONTROL );
    Set_motor_torque_mode(DRIVER_ID_TWO_CONTROL );
    Set_motor_torque_mode(DRIVER_ID_THREE_CONTROL );
    Set_motor_torque_mode(DRIVER_ID_FOUR_CONTROL );
    //设定电机的力矩为零
    Set_torque_value_to_motor(DRIVER_ID_ONE_CONTROL ,0 );
    Set_torque_value_to_motor(DRIVER_ID_TWO_CONTROL ,0 );
    Set_torque_value_to_motor(DRIVER_ID_THREE_CONTROL ,0 );
    Set_torque_value_to_motor(DRIVER_ID_FOUR_CONTROL ,0 );
    //清除超时报警指令
    clear_ALM_motor_driver( DRIVER_ID_ONE_CONTROL );
    clear_ALM_motor_driver( DRIVER_ID_TWO_CONTROL );
    clear_ALM_motor_driver( DRIVER_ID_THREE_CONTROL );
    clear_ALM_motor_driver( DRIVER_ID_FOUR_CONTROL );
    sleep(1);   //等待接收线程稳定就绪
    // 设置使能电机驱动器
    enable_motor_driver(DRIVER_ID_ONE_CONTROL);
    enable_motor_driver(DRIVER_ID_TWO_CONTROL);
    enable_motor_driver(DRIVER_ID_THREE_CONTROL);
    enable_motor_driver(DRIVER_ID_FOUR_CONTROL);

    return 1;

#endif

}



//PID控制
 int16_t  DriverPub::motor_velocityPidCtl_one(float target_speed,float current_speed)
 {
     static float motor_speed_out;
     static float   temp = 0;
    //  static int32_t bias,bias_last,bias_integral = 0;
    //  static float  bias,bias_last,bias_integral =0;

     bias[0] = target_speed - current_speed;                               //积分偏差
                                                                //积分累加
     
    //   printf("bias_integral: %f \n",bias_integral);          
      motor_pid_[0].pid_bias = bias_integral[0];

       //抗积分饱和20200718
    if(motor_speed_out >= MAX)//正向超限
    {
        if(bias[0] < 0)//只累加负偏差
        {
            bias_integral[0] += bias[0];  
        }
    }
    else if(motor_speed_out <= MIN)//反向超限
    {
        if(bias[0] > 0)//只累加正偏差
        {
            bias_integral[0] += bias[0];  
        }
    }
    else
    {
         bias_integral[0] += bias[0];  
    }
    // printf("motor_kp[0]:%f\n",motor_kp[0]);
    // printf("motor_ki[0]:%f\n",motor_ki[0]);
    // printf("motor_kd[0]:%f\n",motor_kd[0]);
    motor_speed_out = motor_kp[0] * bias[0] *PID_SCALE + motor_kd[0] * (bias[0]-bias_last[0])*PID_SCALE  + motor_ki[0] * bias_integral[0] * PID_SCALE;
    // motor_speed_out = motor_speed_out * PID_TEMP;
   //20200718
   if(motor_speed_out > MAX)  //2^14 * 0xFFFF
    {
    	motor_speed_out = MAX;
    }
    else if(motor_speed_out < MIN)
    {
    	motor_speed_out = MIN;
    }
                  
    motor_pid_[0].pid_bias_integral = bias_integral[0];
    bias_last[0] = bias[0];
    
  
      
      temp = motor_speed_out*1;
      if(pid_wheel_==1)
      {
                // motor_pid_[0].motor_kp_ = motor_kp[0];
                // motor_pid_[0].motor_ki_ = motor_ki[0];
                // motor_pid_[0].motor_speed_out_temp = temp;
                motor_pid_[0].set_speed_l_ = target_speed;
                motor_pid_[0].curr_speed_l_ = current_speed;
                speed_pid_.publish(motor_pid_[0]);

      }

      return temp;

 }

  int16_t  DriverPub::motor_velocityPidCtl_two(float target_speed,float current_speed)
 {
    static float motor_speed_out;
    static int32_t   temp = 0;
    //  static float  bias,bias_last,bias_integral =0;

     bias[1] = target_speed - current_speed;
    //  printf("bias[1]:%lf\n",bias[1]); 
        //抗积分饱和20200718
    if(motor_speed_out >= MAX)//正向超限
    {
        if(bias[1] < 0)//只累加负偏差
        {
            bias_integral[1] += bias[1];  
        }
    }
    else if(motor_speed_out <= MIN)//反向超限
    {
        if(bias[1] > 0)//只累加正偏差
        {
            bias_integral[1] += bias[1];  
        }
    }
    else
    {
         bias_integral[1] += bias[1];  
    }
    // printf("bias_integral[1]: %f \n",bias_integral[1]);    
        
    // printf("motor_kp[1]:%f\n",motor_kp[1]);
    // printf("motor_ki[1]:%f\n",motor_ki[1]);
    // printf("motor_kd[1]:%f\n",motor_kd[1]);
    motor_speed_out = motor_kp[1] * bias[1] * PID_SCALE + motor_kd[1] * (bias[1]-bias_last[1]) *PID_SCALE + motor_ki[1] * bias_integral[1] *PID_SCALE ;
//    motor_speed_out = motor_speed_out * PID_TEMP;
    // printf("motor_speed_out: %f \n",motor_speed_out);
   //20200718
   if(motor_speed_out > MAX)  //2^14 * 0xFFFF
    {
    	motor_speed_out = MAX;
    }
    else if(motor_speed_out < MIN)
    {
    	motor_speed_out = MIN;
    }
                  
      bias_last[1] = bias[1];

 if(pid_wheel_==2)
 {
      motor_pid_[0].set_speed_l_ = target_speed;
      motor_pid_[0].curr_speed_l_ = current_speed;
      speed_pid_.publish(motor_pid_[0]);

 }
      return motor_speed_out;
 }

  int16_t  DriverPub::motor_velocityPidCtl_three(float target_speed,float current_speed)
 {
      static float motor_speed_out;
     static float   temp = 1.5;
    //  static float  bias,bias_last,bias_integral =0;

    //   if(target_speed==0 )      //停下车子，轮子协力
    //  {
    //      temp = 0;
    //      return temp;
    //  }
     bias[2] = target_speed - current_speed;
        //抗积分饱和20200718
    if(motor_speed_out >= MAX)//正向超限
    {
        if(bias[2] < 0)//只累加负偏差
        {
            bias_integral[2] += bias[2];  
        }
    }
    else if(motor_speed_out <= MIN)//反向超限
    {
        if(bias[2] > 0)//只累加正偏差
        {
            bias_integral[2] += bias[2];  
        }
    }
    else
    {
         bias_integral[2] += bias[2];  
    }

    // printf("bias_integral[2]: %f \n",bias_integral[2]);    
    // printf("bias[2]:%lf\n",bias[2]);      
    // printf("motor_kp[2]:%f\n",motor_kp[2]);
    // printf("motor_ki[2]:%f\n",motor_ki[2]);
    // printf("motor_kd[2]:%f\n",motor_kd[2]);
     motor_speed_out = motor_kp[2] * bias[2] *PID_SCALE+ motor_kd[2] * (bias[2]-bias_last[2])*PID_SCALE  + motor_ki[2] * bias_integral[2]*PID_SCALE ;
    // printf("motor_speed_out[2]: %f \n",motor_speed_out);
    //  motor_speed_out = motor_speed_out * PID_TEMP;
   //20200718
   if(motor_speed_out > MAX)  //2^14 * 0xFFFF
    {
    	motor_speed_out = MAX;
    }
    else if(motor_speed_out < MIN)
    {
    	motor_speed_out = MIN;
    }
                  
      bias_last[2] = bias[2];
 if(pid_wheel_==3)
 {
     motor_pid_[0].set_speed_l_ = target_speed;
    motor_pid_[0].curr_speed_l_ = current_speed;
    speed_pid_.publish(motor_pid_[0]);
 }

    return motor_speed_out;

 }
  int16_t  DriverPub::motor_velocityPidCtl_four(float target_speed,float current_speed)
 {
     static float motor_speed_out;
     static int32_t   temp = 0;
    //  static float  bias,bias_last,bias_integral =0;
// printf("target_speed: %f  current_speed: %f \n",target_speed,current_speed );
//       if(target_speed==0 )      //停下车子，轮子协力
//      {
//          temp = 0;
//          return temp;
//      }
     bias[3] = target_speed - current_speed;
         //抗积分饱和20200718
    if(motor_speed_out >= MAX)//正向超限
    {
        if(bias[3] < 0)//只累加负偏差
        {
            bias_integral[3] += bias[3];  
        }
    }
    else if(motor_speed_out <= MIN)//反向超限
    {
        if(bias[3] > 0)//只累加正偏差
        {
            bias_integral[3] += bias[3];  
        }
    }
    else
    {
         bias_integral[3] += bias[3];  
    }

    // printf("bias_integral[3]: %f \n",bias_integral[3]);    
    // printf("bias[3]:%lf\n",bias[3]);      
    // printf("motor_kp[3]:%f\n",motor_kp[3]);
    // printf("motor_ki[3]:%f\n",motor_ki[3]);
    // printf("motor_kd[3]:%f\n",motor_kd[3]);
     motor_speed_out = motor_kp[3] * bias[3] *PID_SCALE+ motor_kd[3] * (bias[3]-bias_last[3]) *PID_SCALE + motor_ki[3] * bias_integral[3] *PID_SCALE;
    // printf("motor_speed_out[3]: %f \n",motor_speed_out);
    // motor_speed_out = motor_speed_out * PID_TEMP;
   //20200718
   if(motor_speed_out > MAX)  //2^14 * 0xFFFF
    {
    	motor_speed_out = MAX;
    }
    else if(motor_speed_out < MIN)
    {
    	motor_speed_out = MIN;
    }
                  
      bias_last[3] = bias[3];
   if(pid_wheel_==4)
   {
     motor_pid_[0].set_speed_l_ = target_speed;
      motor_pid_[0].curr_speed_l_ = current_speed;
      speed_pid_.publish(motor_pid_[0]);

   }


      return (int16_t)motor_speed_out;

 }

//动态重配置功能回调函数
void DriverPub::dynamic_reconfig_callback(zeus_s2_driver::PID_reconfigConfig &config,uint32_t level)
{
	ROS_INFO("Set PID P:[%d], I:[%d], D:[%d], ELE:[%d]",config.Kp,config.Ki,config.Kd,config.Ele_curr);
    pid_wheel_ = config.Ele_curr;
    speed_l = config.Speed_l;
    speed_r = config.Speed_r;
    printf("speed_l: %f \n",speed_l);
    //  printf("speed_r: %f \n",speed_r);
if(pid_wheel_ ==1)
{
     motor_kp[0] = config.Kp;
     motor_ki[0] = config.Ki;
     motor_kd[0] = config.Kd;

}else if(pid_wheel_ == 2){
    motor_kp[1] = config.Kp;
    motor_ki[1] = config.Ki;
    motor_kd[1] = config.Kd;

}else if(pid_wheel_ == 3){
    motor_kp[2] = config.Kp;
    motor_ki[2] = config.Ki;
    motor_kd[2] = config.Kd;

}else if(pid_wheel_ == 4){
    motor_kp[3] = config.Kp;
    motor_ki[3] = config.Ki;
    motor_kd[3] = config.Kd;

}
    

     reset_vol = config.bool_param;
     mode_selection = config.mode_param;


}

/*
*       返回心跳计数值
*
*/
uint64_t DriverPub::my_xTaskGetTickCount(void)
{
    return count;

}

//检测通信是否超时中断
//判断故障标志
 void  DriverPub::motor_driver_check(void)
 {
    if((ros::Time::now() - now_[0]).toSec() >= 1)
    {
        ROS_WARN_THROTTLE(1,"Didn't received odom data,Please check your connection!");
        printf("请检查急停按钮和驱动器电源按钮是否按下，如果按下，请先关闭此程序，然后打开急停开关和驱动器电源按钮并重新启动此程序\n");
        motor_ctlVol_one = 0;
        motor_ctlVol_two = 0;
        motor_ctlVol_three = 0;
        motor_ctlVol_four = 0;
		driver_count = 11;

    }
    for(int i=0;i<4;i++){
        if(driverFaultCode[i]!=0)
        {
            printf("driverFaultCode[%d]:%d\n",i,driverFaultCode[i]);
            driver_count++;
        }else{
            // driver_count = 0;
        }

    }
    
    if(driver_count>10)
    {
        printf("驱动器故障警告，停止任务\n");
        ros::shutdown();
        exit(1);
    }
   

     if(count > 200)            //8ms *200 = 1600ms
     {
        if(queueFlag != 0x0F)
        {
                while(ros::ok()){
                printf("电机驱动器失联，请检查驱动器硬件通讯连接\n");
                motor_ctlVol_one = 0;
                motor_ctlVol_two = 0;
                motor_ctlVol_three = 0;
                motor_ctlVol_four = 0;
             /*发送电机转距值*/
            Set_torque_value_to_motor(DRIVER_ID_ONE_CONTROL ,motor_ctlVol_one );
            Set_torque_value_to_motor(DRIVER_ID_TWO_CONTROL ,motor_ctlVol_two );
            Set_torque_value_to_motor(DRIVER_ID_THREE_CONTROL ,motor_ctlVol_three );
            Set_torque_value_to_motor(DRIVER_ID_FOUR_CONTROL ,motor_ctlVol_four );
            printf("停止任务\n");
            ros::shutdown();
            // exit(1);
            }
                
        }
        queueFlag = 0;
         count = 0;
     }


 }


//主循环
void DriverPub::loop()
{

    ros::NodeHandle n("~");
    ros::Rate loop_rate(125);

    //获取配置参数
    n.param<std::string>("version",code_version_,std::string("V1.0.0"));
    n.param<std::string>("port_name",port_name_,std::string("/dev/ttyUSB0"));   //驱动器串口
    n.param<int>("baud_rate",baud_rate_,115200);
	// n.param<std::string>("odom_frame",odom_frame_,std::string("odom"));
    n.param<std::string>("base_frame",base_frame_,std::string("base_footprint"));
    n.param<double>("encoder_resolution_4wd",encoder_resolution_4wd_,10000);
    n.param<double>("wheel_track_4wd",wheel_track_4wd_,0.56);  
    n.param<double>("wheel_track",wheel_track_,0.56);  
    n.param<double>("linear_correction_factor",linear_correction_factor_,1.0);
    n.param<double>("angular_correction_factor",angular_correction_factor_,1.0);
    n.param<double>("gear_reduction_4wd",gear_reduction_4wd_,40);
    n.param<double>("wheel_diameter_4wd",wheel_diameter_4wd_,0.305);
    n.param<double>("start_rotation_limit_w",start_rotation_limit_w_,0.4);								//角速度限速
    n.param<bool>("start_flag",start_flag_,true);
    n.param<int>("encoder_min",encoder_min_,-2147483648);
    n.param<int>("encoder_max",encoder_max_,2147483647);
    // n.param<double>("encoder_low_wrap",encoder_low_wrap_,(encoder_max_-encoder_min_)*0.3 + encoder_min_);
    // n.param<double>("encoder_high_wrap",encoder_high_wrap_,(encoder_max_-encoder_min_)*0.7 + encoder_min_);
    n.param<int>("l_wheel_mult",l_wheel_mult_,0);
    n.param<int>("r_wheel_mult",r_wheel_mult_,0);
    // n.param<bool>("publish_odom_transform",publish_odom_transform_,false);
    n.param<float>("Kp_0",motor_kp[0],0.0);
    n.param<float>("Ki_0",motor_ki[0],0.0);
    n.param<float>("Kd_0",motor_kd[0],0.0);

     n.param<float>("Kp_1",motor_kp[1],0.0);
    n.param<float>("Ki_1",motor_ki[1],0.0);
    n.param<float>("Kd_1",motor_kd[1],0.0);

     n.param<float>("Kp_2",motor_kp[2],0);
    n.param<float>("Ki_2",motor_ki[2],0);
    n.param<float>("Kd_2",motor_kd[2],0);

     n.param<float>("Kp_3",motor_kp[3],0);
    n.param<float>("Ki_3",motor_ki[3],0);
    n.param<float>("Kd_3",motor_kd[3],0);

    n.param<int>("Ele_curr",ele_currVol,0);
    n.param<float>("Speed_l",speed_l,0);
    n.param<float>("Speed_r",speed_r,0);


  
   

    encoder_low_wrap_ = ((encoder_max_-encoder_min_)*0.3 + encoder_min_);
    encoder_high_wrap_ = ((encoder_max_-encoder_min_)*0.7 + encoder_min_);
    //计算里程计关键参数
    encoder_resolution_4wd_ = encoder_resolution_4wd_/linear_correction_factor_;        //脉冲数10000 / 1.0
    wheel_track_4wd_ = wheel_track_4wd_/angular_correction_factor_;                                 //轮距

    ticks_per_meter_ =( encoder_resolution_4wd_ * gear_reduction_4wd_)/(wheel_diameter_4wd_ * M_PI); //(10000 * 40)/(0.305*PI)==脉冲数/m
    // speedTemp  = gear_reduction_4wd_*wheel_diameter_4wd_*PI/60.0;     //单位m/s
    // printf("speedTemp:%lf\n",speedTemp);
    dynamic_reconfigure::Server<zeus_s2_driver::PID_reconfigConfig> reconfig_server;
	dynamic_reconfigure::Server<zeus_s2_driver::PID_reconfigConfig>::CallbackType f;

	f = boost::bind(&DriverPub::dynamic_reconfig_callback,this,_1,_2);
	reconfig_server.setCallback(f);
    boost::thread recv_thread(boost::bind(&DriverPub::receive_func,this));
	sleep(1);
	if(initRobot())
    {
        // odom_pub_ = n.advertise<nav_msgs::Odometry>("/odom", 10);
        //发布电机的电流0.01A、速度/rpm、位置/脉冲
        wheel_state_feedback_=n.advertise<zeus_s2_msgs::Feedback>("/motor_driverFeedback",10);
        cmd_sub_ = n.subscribe<geometry_msgs::Twist>("/zeus_speed",1,&DriverPub::cmd_vel_callback,this);      //接受上层下发的速度指令
        // cmd_joy_moment_ = n.subscribe<zeus_s2_msgs::joy_control>("/cmd_moment",3,&DriverPub::cmd_joy_callback,this);      //接受ps3下发的力矩遥控指令
        speed_pid_ = n.advertise<zeus_s2_msgs::motor_pid_vol>("/motor_speed_pid",10);
        // timer1 = n.createTimer(ros::Duration(1.0),callback1);           // timer1每1.0s触发一次callback1函数
         //初始化USB—CAN线程，接受数据
        // boost::thread recv_thread(boost::bind(&DriverPub::receive_func,this));

        //创建一个定时线程
        //定时判断接收标志是否被 置位，如果置位，说明驱动器通信健康，否则，设定故障标志，进入故障程序，控制电机力矩值为0
        // if(regularHealthExamination!=1)
        // {
        //     //设置故障标志==1
        //     CommunicationFault_Sign = 1;

        // }
        // //清标志
        // regularHealthExamination = 0;

        now_[0] = ros::Time::now();
        now_[1] = ros::Time::now();
        now_[2] = ros::Time::now();
        now_[3] = ros::Time::now();
        motor_ctlVol_one = 0;
        motor_ctlVol_two = 0;
        motor_ctlVol_three = 0;
        motor_ctlVol_four = 0;
         left_right_wheel_state_. front_left_encoder.velocity = 0;
         left_right_wheel_state_. front_right_encoder.velocity = 0;
         left_right_wheel_state_. rear_right_encoder.velocity  = 0;
         left_right_wheel_state_. rear_left_encoder.velocity  = 0;
         motorSpeed[0] = 0;
         motorSpeed[1] = 0;
         motorSpeed[2] = 0;
         motorSpeed[3] = 0;

        while(ros::ok())
        {     
            /*遥控模式*/
            if(joy_flag_){
                 if(ps3_control_.priority_flag){
                 joy_flag_ = 0;
                 continue;
             }
          
             /*速度模式下*/
             if(ps3_control_.parking_mode==1.0){
                    mode_isnot = true;
                    switching_mode = DRIVER_PARKING_MODE;
                    state_temp_ = DRIVER_SWITCHING_MODE;
                    ps3_control_.parking_mode = 0;
             }
             else if(ps3_control_.parking_mode == 2.0){         //力矩模式下
                    mode_isnot = true;
                    switching_mode = DRIVER_OPERATION_MODE;
                    state_temp_ = DRIVER_SWITCHING_MODE;
                    ps3_control_.parking_mode = 0;
                
             }
             mode_switching_state_machine(switching_mode, mode_isnot);
            }
            else{    /*非遥控模式*/

         
           
#if 0
            if(reset_vol){
                /*设置速度模式*/
                Set_motor_speed_mode(DRIVER_ID_TWO_CONTROL );
                /*设置速度值*/
                Set_speed_value_to_motor(DRIVER_ID_TWO_CONTROL ,0 );
               
            }
            else if(mode_selection){
                         /*设定电机力矩模式*/
                        Set_motor_torque_mode(DRIVER_ID_TWO_CONTROL );
                         /*设定电机的力矩为零*/
                        Set_torque_value_to_motor(DRIVER_ID_TWO_CONTROL ,0 );
                        // reset_vol = 0;
            }
#endif     
           
            /*******************************************************/
               /*测试模式下，用做pid调节实验*/
            if(reset_vol){
                left = speed_l;
                right = -speed_l;
                if(pid_wheel_ ==1)
                {
                         motor_ctlVol_one =  motor_velocityPidCtl_one(left,  left_right_wheel_state_. front_left_encoder.velocity);                  //m/s
                          Set_torque_value_to_motor(DRIVER_ID_ONE_CONTROL ,motor_ctlVol_one );
                          printf("调节1号电机PID\r\n");
                }
                else if(pid_wheel_ ==2){
                         motor_ctlVol_two =  motor_velocityPidCtl_two(right , left_right_wheel_state_. front_right_encoder.velocity);
                        Set_torque_value_to_motor(DRIVER_ID_TWO_CONTROL ,motor_ctlVol_two );
                        printf("调节2号电机PID\r\n");
                } else if(pid_wheel_ ==3){
                        motor_ctlVol_three = motor_velocityPidCtl_three(right ,left_right_wheel_state_. rear_right_encoder.velocity );
                         Set_torque_value_to_motor(DRIVER_ID_THREE_CONTROL ,motor_ctlVol_three );
                         printf("调节3号电机PID\r\n");
                } else if(pid_wheel_ ==4){
                         motor_ctlVol_four = motor_velocityPidCtl_four(left,left_right_wheel_state_. rear_left_encoder.velocity );  
                         Set_torque_value_to_motor(DRIVER_ID_FOUR_CONTROL ,motor_ctlVol_four );
                         printf("调节4号电机PID\r\n");
                }              
               
                wheel_state_feedback_.publish(left_right_wheel_state_);
                ros::spinOnce();
                loop_rate.sleep();
                continue;
            }
            
         }

            send_speed_callback();              //将cmd_vel下发的速度处理后，用于发送给驱动器
           

            // printf("################################\r\n");
            /*******************************************************/
                /*pid控制*/
            motor_ctlVol_one =  motor_velocityPidCtl_one(left,  left_right_wheel_state_. front_left_encoder.velocity);                  //m/s
            motor_ctlVol_two =  motor_velocityPidCtl_two(right , left_right_wheel_state_. front_right_encoder.velocity);
            motor_ctlVol_three = motor_velocityPidCtl_three(right ,left_right_wheel_state_. rear_right_encoder.velocity );
            motor_ctlVol_four = motor_velocityPidCtl_four(left,left_right_wheel_state_. rear_left_encoder.velocity );    
           
            motor_driver_check();                 //检测驱动器通信问题

            /*发送电机电流闭环控制*/
            Set_torque_value_to_motor(DRIVER_ID_ONE_CONTROL ,motor_ctlVol_one );
            Set_torque_value_to_motor(DRIVER_ID_TWO_CONTROL ,motor_ctlVol_two );
            Set_torque_value_to_motor(DRIVER_ID_THREE_CONTROL ,motor_ctlVol_three );
            Set_torque_value_to_motor(DRIVER_ID_FOUR_CONTROL ,motor_ctlVol_four );
            wheel_state_feedback_.publish(left_right_wheel_state_);
              ++count;
              ros::spinOnce();
            loop_rate.sleep();
        }
    Set_torque_value_to_motor(DRIVER_ID_ONE_CONTROL ,0 );
    Set_torque_value_to_motor(DRIVER_ID_TWO_CONTROL ,0 );
    Set_torque_value_to_motor(DRIVER_ID_THREE_CONTROL ,0 );
    Set_torque_value_to_motor(DRIVER_ID_FOUR_CONTROL ,0 );
    printf("结束线程！！！！！！！！！！！！！！！！！\n");
    }


}

/*
*       运动模式和驻车模式逻辑切换状态机
*
*
*/
void DriverPub::mode_switching_state_machine(uint8_t state, bool isnot)
{

        if(isnot){
            
            switch (state_temp_)
            {
            case DRIVER_SWITCHING_MODE:
                /*清空接收标志变量*/
                mode_rev_flag = 0;
                /*判断进入模式，并发送对应的模式指令*/
                if(state == DRIVER_OPERATION_MODE){
                      /*设定电机力矩模式*/
                        Set_motor_torque_mode(DRIVER_ID_ONE_CONTROL );
                        Set_motor_torque_mode(DRIVER_ID_TWO_CONTROL );
                        Set_motor_torque_mode(DRIVER_ID_THREE_CONTROL );
                        Set_motor_torque_mode(DRIVER_ID_FOUR_CONTROL );
                         /*设定电机的力矩为零*/
                        Set_torque_value_to_motor(DRIVER_ID_ONE_CONTROL ,0 );
                        Set_torque_value_to_motor(DRIVER_ID_TWO_CONTROL ,0 );
                        Set_torque_value_to_motor(DRIVER_ID_THREE_CONTROL ,0 );
                        Set_torque_value_to_motor(DRIVER_ID_FOUR_CONTROL ,0 );
                        state_temp_ = DRIVER_TIMEOUT_DETECTION;
                      
                }
                 if(state == DRIVER_PARKING_MODE){
                       /*设置速度模式*/
                    Set_motor_speed_mode(DRIVER_ID_ONE_CONTROL );
                    Set_motor_speed_mode(DRIVER_ID_TWO_CONTROL );
                    Set_motor_speed_mode(DRIVER_ID_THREE_CONTROL );
                    Set_motor_speed_mode(DRIVER_ID_FOUR_CONTROL );
                     /*设置速度值*/
                    Set_speed_value_to_motor(DRIVER_ID_ONE_CONTROL ,0 );
                    Set_speed_value_to_motor(DRIVER_ID_TWO_CONTROL ,0 );
                    Set_speed_value_to_motor(DRIVER_ID_THREE_CONTROL ,0 );
                    Set_speed_value_to_motor(DRIVER_ID_FOUR_CONTROL ,0 );
                    state_temp_ = DRIVER_TIMEOUT_DETECTION;
                     
                }
                /*进入超时判断状态*/
                /*置位时间函数*/
                time_setMana(TIME_DRIVER_MODE);
                //  printf("-----------------------------------------------------------------------------\n");

                break;
            case DRIVER_OPERATION_MODE:
                /*运行模式下，灯光状态*/
                mode_isnot = false;
                state_temp_ = DRIVER_SWITCHING_MODE;
                break;
            case DRIVER_TIMEOUT_DETECTION:
                /*判断时间是否超时  8ms*100 ==400ms*/
                if(time_checkHandle(TIME_DRIVER_MODE, 100)){
                    /*打印错误提示，可使用灯光报警*/
                    printf("模式切换超时！！\n");
                    mode_isnot = false;
                }
                else{/*未超时*/
                    /*判断模式接收标志是否完成接收，完成接收，进入对应的状态模式*/
                    if(mode_rev_flag==0x0F){
                        if(state == DRIVER_OPERATION_MODE){
                                state_temp_ = DRIVER_OPERATION_MODE;
                        }
                        else if(state == DRIVER_PARKING_MODE){
                                state_temp_ = DRIVER_PARKING_MODE;
                        }
                       
                    }
                }
                break;
            case DRIVER_PARKING_MODE:
                 /*运行模式下，灯光状态*/
                mode_isnot = false;
                state_temp_ = DRIVER_SWITCHING_MODE;
                break;
            
            default:
                break;
            }
        }

}

/*
*       采集心跳包时间计数
*
*
*/
void  DriverPub::time_setMana(unsigned char  time_id)
{
        timeArray[time_id] = my_xTaskGetTickCount();

}

/*
*       检查时间是否达到
*       返回值：
*       1   :   时间到
*       0   ： 时间未到达
*/
unsigned char  DriverPub::time_checkHandle(unsigned char  time_id, unsigned short time_ms)
{
    if((my_xTaskGetTickCount() - timeArray[time_id]) >= time_ms){
        return 1;
    }
    else
    {
        return 0;
    }

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "Driver_s2_node");
    DriverPub driver_s2_usb;
    driver_s2_usb.loop();
 
    return 0;
}
